% plot_smoothness

function [torque_vector, output, J_opt, J_fsolve] = plot_smoothness(x)

% clear('all');
close('all');
addpath('../animations', '../controllers', '../dynamics', '../environments', '../library');

fp_options = optimset('Display','iter','TolFun',1e-14, 'TolX',1e-14);

tic
td_angle = fsolve(@fp_fun,  evalin('base','first_guess'), fp_options);
% error = fp_fun(td_angle)
toc

assignin('base','td_angle',td_angle);

% anim = AnimateForceControlModelClass(sim.gnd, output);
% anim.animate;

close all


NUM_DISCRETE_TORQUES = evalin('base', 'NUM_DISCRETE_TORQUES');
NUM_TIME_WINDOWS = evalin('base','NUM_TIME_WINDOWS');
MAX_STANCE_TIME = evalin('base','MAX_STANCE_TIME');

torque_vector = evalin('base','initial_torque_vector'); %6*ones(evalin('base','NUM_DISCRETE_TORQUES'),1);
time_vector = evalin('base','initial_time_vector');


A = [];
B = [];
Aeq = [zeros(1,NUM_DISCRETE_TORQUES), ones(1,NUM_TIME_WINDOWS)];
    Beq = [MAX_STANCE_TIME];
    lb = [-inf(NUM_DISCRETE_TORQUES,1); zeros(NUM_TIME_WINDOWS,1)];
    ub = [inf(NUM_DISCRETE_TORQUES,1); MAX_STANCE_TIME*ones(NUM_TIME_WINDOWS,1)];
    
    
    
    % to_options = optimset('Display','iter','TolFun',1e-15, 'TolX',1e-10);
    
    
    % x = fsolve(@basiccon,  torque_vector, to_options);
%     x = [torque_vector; time_vector];
    
   
check_index = 1;
pert_list = linspace(-0.0001, 0.0001, 25);

for iter = 1:numel(pert_list)
    pert_vec = zeros(23,1);
    pert_vec(check_index) = pert_list(iter);
    J = obj_fun(x+pert_vec);
    J_vec(iter) = J;
    [C, Ceq] = nonlcon(x+pert_vec);
    Ceq1_vec(iter) = Ceq(1);
    Ceq2_vec(iter) = Ceq(2);
end

subplot(1,3,1)
plot(pert_list, J_vec, 'kx-')
grid on
subplot(1,3,2)
plot(pert_list, Ceq1_vec, 'rx-')
grid on
subplot(1,3,3)
plot(pert_list, Ceq2_vec, 'bx-')
grid on
    
%     J_fsolve = obj_fun(x)
    

    
%     opt_options = optimset('Display','iter','Algorithm','sqp', 'TolFun',1e-3, 'TolX',1e-3);
    
%     [torque_vector, J_opt] = fmincon(@obj_fun, x, A, B, Aeq, Beq, lb, ub, @nonlcon, opt_options);
    
%     [output, sim] = singlestepsim(torque_vector);
    
    
    % figure;
    % title('CoM Height');
    % xlabel('Time (s)');
    % ylabel('Height (m)');
    % hold on;
    % grid on
    % for i = 1 : phases
    %     plot(output{i, 1}, output{i, 2}(:, sim.yci), 'linewidth', 3);
    % end
    %
    % figure;
    % title('CoM Velocity');
    % xlabel('Time (s)');
    % ylabel('Velocity (m/s)');
    % hold on;
    % grid on
    % for i = 1 : phases
    %     plot(output{i, 1}, output{i, 2}(:, sim.dxci), 'linewidth', 3);
    % end
    %
    %
    % figure;
    % title('Gear Angle');
    % xlabel('Time (s)');
    % ylabel('Angle');
    % hold on;
    % grid on
    % for i = 1 : phases
    %     plot(output{i, 1}, output{i, 2}(:, sim.oli), 'linewidth', 3);
    % end
    %
    % close all
    
    % anim = AnimateForceControlModelClass(sim.gnd, output);
    %         anim.animate;
    
    
    
    %% Fixed Point function
    
    function error = fp_fun(x)
    
    phases = 3;
    output = cell(phases, 2);
    
    x0 = [ 0, evalin('base','x_dot'), evalin('base','y'), 0, 0, 0, x, 0, ForceControlModelClass.r0 * ForceControlModelClass.G / ForceControlModelClass.rl, 0, 0];
    
    % controller = ComControllerClass;
    controller = SimpleControllerClass;
    
    sim = ForceControlModelClass(x0, @flat_rigid_environment, controller);
    
    for i = 1 : phases
        [output{i, 1}, output{i, 2}] = sim.run();
    end
    
    error = [   output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci);    % horizontal velocity
        output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci)       ];      % COM height
    
    % anim = AnimateForceControlModelClass(sim.gnd, output);
    % anim.animate;
    
    end



%% SingleStepSim

    function [output, sim] = singlestepsim(x)
        
        
        
        phases = 3;
        output = cell(phases, 2);
        
        x0 = [ 0, evalin('base','x_dot'), evalin('base','y'), 0, 0, 0, evalin('base','td_angle'), 0, ForceControlModelClass.r0 * ForceControlModelClass.G / ForceControlModelClass.rl, 0, 0];
        
        controller = OptTorqueControllerClass;
        
        NUM_DISCRETE_TORQUES = evalin('base', 'NUM_DISCRETE_TORQUES');
        NUM_TIME_WINDOWS = evalin('base','NUM_TIME_WINDOWS');
        
        controller.time_vector = cumsum([0; x(NUM_DISCRETE_TORQUES+1:NUM_DISCRETE_TORQUES+NUM_TIME_WINDOWS)]);
        controller.torque_vector = x(1:NUM_DISCRETE_TORQUES);
        
        sim = ForceControlModelClass(x0, @dissipative_environment, controller);
        
        for i = 1 : phases
            [output{i, 1}, output{i, 2}] = sim.run();
        end
        
        %         close all
        
        %                 figure(1)
        %                 plot(controller.time_vector, controller.torque_vector,'bx-')
        %                 pause(0.05)
        
        %         figure(2)
        %         anim = AnimateForceControlModelClass(sim.gnd, output);
        %         anim.animate;
        
%         plot(controller.time_vector, controller.torque_vector, 'kx-')
        pause(0.02)
        
    end

%% Objective Function

    function J = obj_fun(x)
        
        try
            [output, sim] = singlestepsim(x);
            
            t = output{2,1};
            x = output{2,2};
            
            
            r = sqrt( ( x(:,sim.yci) - x(:,sim.yti) ).^2 + ...
                ( x(:,sim.xci) - x(:,sim.xti) ).^2 );
            
            power = sim.kl .* r .* sim.rl .* x(:,sim.doli) ./ sim.G ...
                - sim.kl .* sim.rl^2 .* x(:,sim.oli) .* x(:,sim.doli) ./ sim.G^2;
            
            % Operation to ignore all negative work
%             power = (power < 0).*power;
            

            % Smoothing the integrand for SQP convergence (Srinivasan 2010)
            % smoothing parameter: epsilon 
            epsilon = 0.1;
            power = sqrt(power.^2 + epsilon^2)-epsilon;

            J = trapz(t, abs(power));
            
            %         clf
            %         plot(t, x(:,sim.oli))
            
            J
        catch
            J = realmax
        end
        
               
    end

%% Nonlinear Constraint Function

    function [C, Ceq] = nonlcon(x)
        
        try
            [output, sim] = singlestepsim(x);
            
            C = [0; 0];
            
            %             Ceq = [   sign((output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci)))*sqrt(sqrt(abs((output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci)))));
            %                 sign(output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci))*sqrt(sqrt(abs(output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci))))       ];
            
%             Ceq = [   sign((output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci)))*(abs((output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci))))^2;
%                 sign(output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci))*(abs(output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci)))^1       ];

            Ceq = [   output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci);
                (output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci))       ];

            Ceq_debug = [   output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci);
                (output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci))       ]
            
            
        catch
            
            C = [0; 0];
            Ceq = [realmax; realmax]
        end
        
    end



%% Basic Equilibrium Constraint

    function [Ceq] = basiccon(x)
        
        try
            [output, sim] = singlestepsim(x);
            
            %         C = [0; 0];
            %             Ceq = [   sign((output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci)))*sqrt(sqrt(abs((output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci)))));
            %                 sign(output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci))*sqrt(sqrt(abs(output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci))))       ];
            
            Ceq = [   sign((output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci)))*(abs((output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci))))^2;
                sign(output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci))*(abs(output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci)))^1       ];
            
            Ceq_debug = [   output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci);
                (output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci))       ]
        catch
            Ceq = [realmax; realmax]
        end
        
    end

end